function points = triangulateset(cameras, observations)
% points = TRIANGULATESET(cameras, observations) Triangulate a set of points from a set of cameras and their projections
%   Given a set of camera matrices in the form: cameras = [P1; P2; P3; P4; ...],
%   where each camera matrix is of the form: P = K*M, M = [R | -RC],
%   and a set of observations of the form: observations = [p1; p2; p3; p4; ...],
%   where each observation is of the form: p = [ii jj x y], corresponding to 
%   camera index, point index, x projection and y projection (normalised),
%   generates the corresponding point set: points = [X1; X2; X3; X4; ...],
%   where each X is of the form X = [X Y Z 1] corresponding to X,Y,Z position respectively.

% Copyright (c) Michael Warren 2014

% This file is part of the AMMCC Toolbox.

% The AMMCC Toolbox is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
% 
% The AMMCC Toolbox is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU Lesser General Public License for more details.
% 
% You should have received a copy of the GNU Lesser General Public License
% along with the AMMCC Toolbox.  If not, see <http://www.gnu.org/licenses/>.

    % Generate empty vectors to store the output
    pointslist = unique(observations(:,2));
    numpoints = size(pointslist,1);
    points = zeros(numpoints,3);
    
    % For each 3D point we expect to triangulate
    for ii = 1:1:numpoints
        obs = observations(observations(:,2) == pointslist(ii),:);
        obssize = size(obs,1);
        ll = 0;
        clear Z;
        % grab each observation and add it to the matrix to solve
        for jj = 1:1:obssize
            kk = obs(jj,1);
            P = cameras(:,kk*4+1:kk*4+3+1);
            Z(ll*2+1,:) = obs(jj,3)*P(3,:) - P(1,:);
            Z(ll*2+2,:) = obs(jj,4)*P(3,:) - P(2,:);
            ll = ll + 1;
        end
        % solve the linear system
        [U,S,V] = svd(Z);
        % Extract the triangulated point
        X = V(:,4);
        % normalise
        X = X./X(4);
        % Add to the list
        points(ii,:) = X(1:3);
    end




return